#define F_CPU 20000000UL  // 20 MHz CPU clock
#define	STEPSIZE_ticks 50

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#include "serial.h"

// Global variables
Serialt serial;
uint8_t charIn; 								//USART incoming character
char str[] = "\rN/L/R/F/B> ";
int isEnabled = 1; 
uint32_t pwm_ticks;

ISR (TIMER1_COMPB_vect) // Timer 1 is a 16-bit timer
{
	PINC |= _BV(PC1); // Toggle output pin
	
	for(int i=0; str[i]; i++)
			serial_write(&serial, str[i]);
}

void commMonitor(void)
{
	// Deal with incoming serial bytes here
	if (serial_read(&serial,&charIn)) //if there's data read it
	{
		serial_write(&serial, charIn); //echo it back out for now...
		switch (charIn)
		{
			case 'f':
			//	"Forward" forward one step
				pwm_ticks = pwm_ticks + STEPSIZE_ticks; 	//	Leftmost position
				OCR1BH = pwm_ticks >> 8;
				OCR1BL = pwm_ticks & 0xFF;
				break;
			case 'b':
			//	"Back" back one step
				pwm_ticks = pwm_ticks - STEPSIZE_ticks; 	//	Leftmost position
				OCR1BH = pwm_ticks >> 8;
				OCR1BL = pwm_ticks & 0xFF;
				break;
			case 'l':
			//	"Left" go to leftmost position
				pwm_ticks = 1250; 	//	Leftmost position
				OCR1BH = pwm_ticks >> 8;
				OCR1BL = pwm_ticks & 0xFF;
				break;
			case 'r':
			//	"Right" go to full right position
				pwm_ticks = 2500;	//	Rightmost position
				OCR1BH = pwm_ticks >> 8;
				OCR1BL = pwm_ticks & 0xFF;
				break;
			case 'n':
			//	"Neutral" Go to neutral position
				pwm_ticks = 1875; 	// 	Neutral position
				OCR1BH = pwm_ticks >> 8;
				OCR1BL = pwm_ticks & 0xFF;
				break;
			default:
				//	Throw error?
				break;
		}

	}

	return;
}


int main(void) 
{
	
	// Initialization
	
	// 	Set up serial port
	serial_init(&serial);
	
	//	Set up IO pin
	DDRC |= _BV(PC1); 	// set led pin to output direction
	PORTC |= _BV(PC1); 	// led on, pin high
	DDRB |= _BV(PB2);	// enable pwm output pin
	
	//	Set up timer
	TCCR1A |= _BV(5); //WGM's cleared for PWM phase and freq correct mode, non-inverting output, 
								//Compare Match B
	TCCR1B |= _BV(4) | _BV(1); //Clock prescaler 8
	ICR1H = 0x61; //0x61A8 is 25000 as TOP for 50 Hz
	ICR1L = 0xA8;
	TIMSK1 |= _BV(2); // Enable timer interrupt OCIE1B
	
	//	Set pwm value here
	OCR1BH = 0x07; // 0x0753 is 1875 for 1.5 ms (neutral position)
	OCR1BL = 0x53; 

	// Start timer
	TCNT1H = 0; // Set timer high bits
	TCNT1L = 0; // Set timer low bits

	sei(); // Globally enable interrupts
         
    while (1)                       /* loop forever */
    {
		commMonitor();	// Monitor serial port for incoming bytes
    }
	
	
	return 0;
}
